// Copyright 2014-2015 Isis Innovation Limited and the authors of InfiniTAM
#ifndef _FE_TRACKINGSTATE_H
#define _FE_TRACKINGSTATE_H

#include "../Utils/FELibDefines.h"

#include "FEPose.h"
#include "FEPointCloud.h"

namespace FE
{
	/** \brief
		Stores some internal variables about the current tracking
		state, most importantly the camera pose
		*/
	class FETrackingState
	{
	public:
		/** @brief
			Excerpt of the scene used by the tracker to align
			a new frame.

			This is usually the main result generated by the
			raycasting operation in a FE::FESceneReconstructionEngine.
			*/
		FEPointCloud *pointCloud;

		/// The pose used to generate the point cloud.
		FEPose *pose_pointCloud;

		int age_pointCloud;

		/// Current pose of the depth camera.
		FEPose *pose_d;

		bool requiresFullRendering;

		bool TrackerFarFromPointCloud(void) const
		{
			// if no point cloud exists, yet
			if (age_pointCloud < 0) return true;
			// if the point cloud is older than n frames
			if (age_pointCloud > 5) return true;

			Vector3f cameraCenter_pc = -1.0f * (pose_pointCloud->GetR().t() * pose_pointCloud->GetT());
			Vector3f cameraCenter_live = -1.0f * (pose_d->GetR().t() * pose_d->GetT());

			Vector3f diff3 = cameraCenter_pc - cameraCenter_live;

			float diff = diff3.x * diff3.x + diff3.y * diff3.y + diff3.z * diff3.z;

			// if the camera center has moved by more than a threshold
			if (diff > 0.0005f) return true;

			return false;
		}

		FETrackingState(Vector2i imgSize, MemoryDeviceType memoryType)
		{
			this->pointCloud = new FEPointCloud(imgSize, memoryType);

			this->pose_d = new FEPose();
			this->pose_d->SetFrom(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);

			this->age_pointCloud = -1;
			this->pose_pointCloud = new FEPose();
			this->pose_pointCloud->SetFrom(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);

			requiresFullRendering = true;
		}

		~FETrackingState(void)
		{
			delete pointCloud;
			delete pose_d;
			delete pose_pointCloud;
		}

		// Suppress the default copy constructor and assignment operator
		FETrackingState(const FETrackingState&);
		FETrackingState& operator=(const FETrackingState&);
	};
}

#endif //_FE_TRACKINGSTATE_H
